/*
用蓝牙串口返回当前小车数据，接收上位机的调参或运动控制指令
*/
#include <Arduino.h>
#include "CONTROL.h"
#include "BluetoothSerial.h"
#include "BT_CMD.h"
#include "GET_IMU.h"
BluetoothSerial SerialBT;


void Print_Angel()
{
    //用来向上位机发送当前小车姿态数据
    //设置变量i使程序每循环30次发送一组数据，减少对相应时间的影响
    //虽然在上位机上显示出曲线很酷，但，，好像没啥用
    static char i;
    i++;
    while(i>30)
    {
    SerialBT.print(-JY901Pitch);    //返回当前角度
    SerialBT.print(",");
    SerialBT.println(ExPitch);      //返回期望角度
    i=0;
    }

}
void BtCommand() 
{
    //用来接收上位机指令，接收上位机在线调参，接收运动控制指令（前进转向）
    //这里是用void JY901()函数改的，配合VOFA+在每个数据前加上起始位和标志位就能愉快的用蓝牙串口在线调参和遥控了
    static unsigned char PidTurnBuffer[11];
    static unsigned char PidTurnCnt = 0; 
    PidTurnBuffer[PidTurnCnt++]=SerialBT.read();
    if (PidTurnBuffer[0]!=0x11) 
    {
        PidTurnCnt=0;
        return;
    }
    if (PidTurnCnt<10) {return;}
    else
    {
        switch(PidTurnBuffer[1])
        {
        //0x2% 调参指令
        case(0x21):memcpy(&Kpv,&PidTurnBuffer[2],4);SerialBT.print("速度环P=");SerialBT.println(Kpv);break;
        case(0x22):memcpy(&Kiv,&PidTurnBuffer[2],4);SerialBT.print("速度环I=");SerialBT.println(Kiv);break;
        case(0x23):memcpy(&Kpa,&PidTurnBuffer[2],4);SerialBT.print("平衡环P=");SerialBT.println(Kpa);break;
        case(0x24):memcpy(&Kda,&PidTurnBuffer[2],4);SerialBT.print("平衡环D=");SerialBT.println(Kda);break;
        case(0x25):memcpy(&IntgMax,&PidTurnBuffer[2],4);SerialBT.print("速度环积分上限=");SerialBT.println(IntgMax);break;
        //0x3% 运动控制指令
        case(0x30):memcpy(&SteerSpd,&PidTurnBuffer[2],4);memcpy(&TarSpd,&PidTurnBuffer[6],4);
        SerialBT.print("前进=");SerialBT.println(TarSpd);SerialBT.print("转向=");SerialBT.println(SteerSpd);IntgErrV=0;break;
        }
        PidTurnCnt=0; 
        
    }
}
